COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	(* ppArmError is called by msgDispatch & moveEnd *)
C00005 00003	(* doSay called by Interp *)
C00010 00004	(* doArmmagic called by Interp *)
C00014 00005	(* doFloat called by Interp *)
C00017 ENDMK
C⊗;
(* ppArmError is called by msgDispatch & moveEnd *)

procedure ppArmError(err: errortypes; angle: integer);
 begin
 if err = nopower then
   begin pp20('arm power not on    ',16); ppLine; end
  else if err = devbusy then
   begin pp20('device currently in ',20); pp5('use  ',4); ppLine end
  else
   begin
   case err of
srvdead:   pp10('servo dead',10);
adcdead:   pp10('a/d error ',9);
panicb:    pp20('panic button pushed ',19);
exjtfc:    begin pp20('excessive joint forc',20); ppChar('e'); end;
timout:    pp10('time out  ',8);
paslim:    pp20('joint out of range  ',18);
badpot:    pp20('bad pot on PUMA     ',15);
noarmsol:  pp20('No arm solution     ',16);
nocart:    begin pp20('No Cartesian path ex',20); pp20('ists between these p',20);
		 pp20('ath points.         ',11) end;
timerr:    begin pp20('Specified motion tim',20); pp20('e exceeds capabiliti',20);
		 pp5('es.  ',3) end;
durerr:    begin pp20('Motion overly constr',20); pp20('ained, will ignore g',20);
		 pp20('lobal time constrain',20); pp5('t.   ',2) end;
toolong:   begin pp20('Maximum segment time',20); pp20(' allowed is 32.2 sec',20);
		 pp5('onds.',5) end;
badparm:   pp20('Bad Magic Parameter ',19);
unkmess:   begin pp20('Unknown Message Type',20); pp20(' received from AL!  ',18) end;
nozind:    begin pp20('No Zero Index found ',20); pp20('( PUMA Encoder )    ',16) end;
baddev:    begin pp20('Device can''t perform',20); pp20(' commanded action   ',17) end;
cbound:    begin pp20('ARM Code compute bou',20); pp5('nd!  ',3) end;
featna:    begin pp20('Feature not availabl',20); pp10('e yet.    ',6) end;
otherwise  begin pp20('Unknown error! =    ',17); ppInt(ord(err)) end;
    end;
   badJoints(angle);     (* tell which joint(s) were bad, if any *)
   end;
end;

(* doSay called by Interp *)

procedure doSay;
 var n,np: nodep; b: boolean;

 procedure sayInt(i: integer);
  var j: integer; n: array [1..9] of integer;
  begin
  for j := 1 to 9 do		(* get individual digits *)
   begin n[j] := i mod 10; i := i div 10 end;
  j := 9;
  while (j > 1) and (n[j] = 0) do j := j - 1;	(* ignore leading zeros *)
  for i := j downto 1 do
   write(talk,chr(ord('0')+n[i]));		(* say digit *)
  end;
  
 procedure saySval(s: real);
  var si: real; ip,fp: integer;
  begin
  if s < maxInt then
    begin
    si := trunc(s);
    s := si + round(1000*(s-si))/1000;
    ip := trunc(s);
    fp := trunc(1000*(s-ip));
    sayInt(ip);				(* say integer part *)
    if fp > 0 then
      begin				(* say fractional part too *)
      write(talk,' point ');
      sayInt(fp);
      end;
    end
   else
    begin				(* it's a bignum *)
    fp := 0;
    repeat fp := fp + 1; s := s / 10 until s <= maxint;	(* scale it down *)
    sayInt(trunc(s));			(* say significant digits *)
    for ip := 1 to fp do		(* now the trailing zeros *)
     write(talk,'0');
    end;
  write(talk,' ,, ');			(* add a small pause *)
  end;

 procedure sayVec(v: vectorp);
  var i: integer;
  begin
  write(talk,' vector ');
  with v↑ do 
   for i := 1 to 3 do 
    begin
    saySval(val[i]);
    end;
  write(talk,' ,, ');			(* add a small pause *)
  end;

 procedure sayTrans(t: transp);
  var i: integer; v: vectorp;
  begin
  with t↑ do
   begin
   refcnt := refcnt + 1;
   write(talk,' trans rot ');
   v := taxis(t); sayVec(v); relVector(v);
   saySval(tmagn(t));
   write(talk,' , vector ');
   for i := 1 to 3 do
    begin
    saySval(val[i,4]);
    end;
   write(talk,' ,, ');			(* add a small pause *)
   refcnt := refcnt - 1;
   end;
  end;

 procedure sayStrng(length: integer; s: strngp);
  var i,j: integer; cntl: boolean; ch: ascii;
  begin
  j := 1;
  cntl := false;
  for i := 1 to length do
   begin
   ch := s↑.ch[j];
   if cntl then
     begin					(* make it a control char *)
     if ord(ch) >= smallA then
       ch := chr(ord(ch) - ord(' '));		(* convert to uppercase *)
     write(talk,chr(ord(ch) - ord('@')));
     cntl := false;
     end
    else if ch = '\' then cntl := true
    else
     write(talk,ch);
   if j = 10 then begin j := 1; s := s↑.next; end
    else j := j + 1;
   end;
  end;

 begin {doSay}
 with curInt↑ do
  begin				(* say whatever user wants us to *)
  n := spc↑.plist;
  while n <> nil do			(* say everything on the list *)
   begin
   np := getNval(n↑.lval,b);
   if np <> nil then
     begin
     with np↑ do
      case ltype of
 svaltype:  saySval(s);
 vectype:   sayVec(v);
 transtype: sayTrans(t);
 strngtype: sayStrng(length,str);
       end;
     if b then killNode(np);	(* flush used stack entry *)
     end;
   n := n↑.next;
   end;
  if spc↑.plist <> nil then
    begin
    writeln(talk);
    break(talk);		(* say it now *)
    end;
  mode := 0;
  spc := spc↑.next;
  end;
 end;

(* doArmmagic called by Interp *)

procedure doArmmagic;
 var e: enventryp; ev: eventp; np: nodep; i,j,k: integer;
 begin
 with curInt↑ do
  case mode of
1:  begin
    np := pop;
    i := round(np↑.s);		(* get # of arm magic command *)
    relNode(np);
    e := gtVarn(spc↑.dev);	(* remember what we're moving *)
    mech := e↑.f;
    ev := getEvent;	(* event to use for signalling when motion finishes *)
    ev↑.count := -1;
    ev↑.waitlist := curInt;
    j := 0;
    np := spc↑.iargs;
    while np <> nil do begin np := np↑.next; j := j + 1 end;	(* count args *)
    with msg↑ do
     begin
     cmd := armmagiccmd;
     n := i;				(* command number *)
     dev := getMechbits;
     bits := j;
     evt := ev;
     sendCmd;				(* initiate the armmagic operation *)
     for i := 1 to j do
      begin				(* send over the arguments *)
      np := pop;			(* get next argument *)
      if np↑.ltype = svaltype then
	begin
	cmd := realcmd;
	dur := np↑.s
	end
       else if np↑.ltype = vectype then
	begin
	cmd := vectorcmd;
	with np↑.v↑ do
	 begin
	 v1 := val[1];			(* copy vector *)
	 v2 := val[2];
	 v3 := val[3];
	 end
	end
       else if np↑.ltype = transtype then
	begin
	cmd := transcmd;
	with np↑.t↑ do
	 begin
	 for k := 1 to 3 do begin t[k] := val[k,1]; t[k+3] := val[k,2] end;
	 sendCmd;			(* send first packet of trans over *)
	 for k := 1 to 3 do begin t[k] := val[k,3]; t[k+3] := val[k,4] end;
	 end;
	end
       else
	begin			(* error -- must be string type *)
	pp20L('ARM MAGIC can''t hand',20); pp10('le strings',10); ppLine;
	cmd := realcmd;
	dur := 0.0;			(* send a zero instead *)
	end;
      sendCmd;			(* send real/vector/2nd-half-of-trans over *)
      killNode(np);			(* flush used stack entry *)
      end;
     end;
    signalArm;				(* start things happening *)

    mode := 2;
    status := devicewait;
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;

2:  begin
    mode := 0;				(* get ready for next statement *)
    spc := spc↑.next;
    end

   end;

end;

(* doFloat called by Interp *)

procedure doFloat;
 var mechbits: integer; e: enventryp; cl,load,val1: nodep; b: boolean;
 begin
 with curInt↑ do
  begin
  load := nil;
  cl := spc↑.clauses;
  while cl <> nil do			(* look for LOAD clause *)
   with cl↑ do
    begin
    if ntype = loadnode then load := cl;
    cl := next;
    end;

  if spc↑.cf = nil then mechbits := GARMDEV		(* assume GARM *)
   else
    begin
    e := gtVarn(spc↑.cf);		(* see what we're floating *)
    with e↑.f↑ do
     if ftype then
       if dev <> nil then mechbits := dev↑.mech
	else
	 begin		(* yow! frame that's not affixed to a device *)
	 pp20L('Attempt to float a f',20); pp20('rame not affixed to ',20);
	 pp20('any device: Assuming',20); pp5(' GARM',5); ppLine;
	 mechbits := GARMDEV;
	 end
      else mechbits := mech;
    end;

  if load <> nil then
    with msg↑ do				(* indicate load for arm *)
     begin
     cmd := setloadcmd;
     if load↑.lcsys then bits := FTABLE	(* in World or Hand? *)
      else bits := FHAND;
     val1 := getNval(load↑.loadval,b);		(* mass of load *)
     dur := val1↑.s;
     if b then relnode(val1);
     if load↑.loadvec <> nil then
       begin
       val1 := getNval(load↑.loadvec,b);	(* where load is located *)
       with val1↑.v↑ do
	begin v1 := val[1]; v2 := val[2]; v3 := val[3] end;
       if b then relnode(val1);
       end
      else begin v1 := 0; v2 := 0; v3 := 0 end;
     sendCmd;					(* tell ARM about the load *)
     end;

  with msg↑ do
   begin
   cmd := floatcmd;
   if load <> nil then bits := Loadcb else bits := 0;
   dev := mechbits;
   end;
  beep;		(* beep the terminal to warn that a float is about to start *)
  sendCmd;				(* tell arm servo to float device *)

  mode := 0;
  spc := spc↑.next;
  end;
 end;